/*=========================================================================

  Program:   Image Guided Surgery Software Toolkit
  Module:    OpenIGTLinkTrackingBroadcaster.h
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) ISC  Insight Software Consortium.  All rights reserved.
  See IGSTKCopyright.txt or http://www.igstk.org/copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#ifndef __OpenIGTLinkTrackingBroadcaster_h
#define __OpenIGTLinkTrackingBroadcaster_h

// BeginLatex
// 
// This example illustrates how to export tracking data through OpenIGTLink
// connection. The example program supports multi-cast data transfer, and is
// the client part of the OpenIGTLink client-server approach.
//
// EndLatex
#include "igstkTrackerConfiguration.h"
#include "igstkTrackerController.h"
#include "igstkPolarisVicraConfigurationXMLFileReader.h"
#include "igstkPolarisSpectraConfigurationXMLFileReader.h"
#include "igstkPolarisHybridConfigurationXMLFileReader.h"
#include "igstkAuroraConfigurationXMLFileReader.h"
#include "igstkMicronConfigurationXMLFileReader.h"
#include "igstkAscensionConfigurationXMLFileReader.h"
#include "igstkAscension3DGConfigurationXMLFileReader.h"
#include "igstkOpenIGTLinkTrackerConfigurationFileReader.h"
#include "igstkTransformObserver.h"

// 
// BeginLatex
// 
// To use the OpenIGTLink POSITION message, and client socket we need the
// following includes:
//
// EndLatex
// BeginCodeSnippet
#include "igtlOSUtil.h"
#include "igtlPositionMessage.h"
#include "igtlClientSocket.h"
// EndCodeSnippet


/**
 * \class OpenIGTLinkTrackingBroadcaster
 * \brief This class performs broadcasting of tracking data using the OpenIGTLink
 * communication protocol. 
 *
 * The class is instantiated with the name of the xml file containing the 
 * tracker and OpenIGTLink information. The user can then start and stop the 
 * tracking.
 * 
 */
class OpenIGTLinkTrackingBroadcaster
{
public:

  /**
   * \class This class was created due to the need for platform independence. 
   * In windows the std::exception class has a constructor which has a string as
   * payload, sadly in linux/unix this constructor does not exist. 
   */
  class ExceptionWithMessage : public std::exception
    {
    public:
    ExceptionWithMessage (const std::string & str) throw()
      {
      this->m_Str = str;
      }
    virtual ~ExceptionWithMessage() throw () {} 

    virtual const char *what() const throw ()
      {
      return this->m_Str.c_str();
      }
    private:
      std::string m_Str;
    };

  OpenIGTLinkTrackingBroadcaster( std::string &trackerXMLConfigurationFileName ) 
    throw ( ExceptionWithMessage );

  ~OpenIGTLinkTrackingBroadcaster(){ }

  void StartTracking();

  void StopTracking();

private:

  /**
  * Observer for the event generated by 
  * TrackerConfigurationFileReader->RequestGetData() method.
  */
  igstkObserverMacro( OpenIGTLinkTrackerConfiguration, 
    igstk::OpenIGTLinkTrackerConfigurationFileReader::
      OpenIGTLinkConfigurationDataEvent, 
    igstk::OpenIGTLinkTrackerConfigurationFileReader::
      OpenIGTLinkConfigurationDataPointerType )

    /**
    * Observer for the TrackerController->RequestInitialize() failure.
    */
    igstkObserverMacro( InitializeError, 
    igstk::TrackerController::InitializeErrorEvent, 
    std::string )


    /**
    * Observer for the TrackerController->RequestStartTracking() failure.
    */
    igstkObserverMacro( StartTrackingError, 
    igstk::TrackerStartTrackingErrorEvent,
    std::string )

    /**
    * Observer for the TrackerController->RequestStopTracking() failure.
    */
    igstkObserverMacro( StopTrackingError, 
    igstk::TrackerStopTrackingErrorEvent,
    std::string )

    /**
    * Observer for the TrackerController->RequestGetNonReferenceToolList() 
    */
    igstkObserverMacro( ToolList, 
    igstk::TrackerController::RequestToolsEvent, 
    igstk::TrackerController::ToolContainerType )

    /**
    * Observer for the TrackerController->RequestGetTool() and  
    * TrackerController->RequestGetReferenceTool()
    */
    igstkObserverMacro( Tool, 
    igstk::TrackerController::RequestToolEvent, 
    igstk::TrackerController::ToolEntryType )

/**
 * This class observes the TrackerToolTransformUpdateEvent for a specific tool.
 * It checks that the event is for the relevant tool and then gets the tool's
 * transform w.r.t. the "world" coordinate system. The transform is then 
 * broadcasted to all destinations.
 */
// BeginLatex
// \code{ToolUpdatedObserver} class observes the
// TrackerToolTransformUpdateEvent for a specific tool. It checks that
// the event is for the relevant tool and then gets the tool's
// transform
// to its parent and sends it via socket.
// EndLatex

  class ToolUpdatedObserver : public ::itk::Command
    {
    public:
      typedef  ToolUpdatedObserver        Self;
      typedef  ::itk::Command             Superclass;
      typedef  ::itk::SmartPointer<Self>  Pointer;
      itkNewMacro( Self );
    protected:
// BeginLatex
// In the \code{ToolUpdatedObserver} constructor  we instantiate a 
// \code{igtl::PositionMessage}, which is updated with the transformation data.
// EndLatex

      ToolUpdatedObserver()
      {
      this->m_TransformObserver = igstk::TransformObserver::New();
// BeginCodeSnippet
      this->m_PositionMessage = igtl::PositionMessage::New();
// EndCodeSnippet
      }

      ~ToolUpdatedObserver()
      {
      std::vector< igtl::ClientSocket::Pointer >::iterator it; 
      for (it = this->m_Sockets.begin(); it != this->m_Sockets.end(); ++it)
        {
        (*it)->CloseSocket();
        (*it)->Delete();
        }
      this->m_Sockets.clear();

      }

    public:

      void Initialize( 
        const std::string toolName, 
        igstk::TrackerTool::Pointer trackerTool,
        igstk::SpatialObject::Pointer world,
        std::vector< std::pair<std::string, unsigned int> > & destinations)
      {
      this->m_Tool = trackerTool;
      this->m_World = world;
      this->m_TransformObserver->ObserveTransformEventsFrom( this->m_Tool );
      // BeginLatex
      // In \emph{ToolUpdatedObserver::Initialize()}, we set the device name
      // of the
      // message by invoking the \emph{SetDeviceName()} method.
      // EndLatex
      // BeginCodeSnippet
      this->m_PositionMessage->SetDeviceName( toolName.c_str() );
      // EndCodeSnippet

      std::vector< igtl::ClientSocket::Pointer >::iterator it; 
      for (it = this->m_Sockets.begin(); it != this->m_Sockets.end(); ++it)
        {
        (*it)->CloseSocket();
        (*it)->Delete();
        }
      this->m_Sockets.clear();

      // BeginLatex
      // We create a list of destinations to support multicast data transfer.
      // Then we establish connections for each destation on the list.
      // EndLatex
      // BeginCodeSnippet
      std::vector< std::pair<std::string, unsigned int> >::iterator
        destinationIt; 
      for ( destinationIt = destinations.begin();
            destinationIt != destinations.end(); ++destinationIt)
        {
        igtl::ClientSocket::Pointer socket = igtl::ClientSocket::New(); 
        int r = socket->ConnectToServer( destinationIt->first.c_str() ,
                                         (int)destinationIt->second );
        if (r != 0)
          {

          for (it = this->m_Sockets.begin(); it != this->m_Sockets.end(); ++it)
            {
            (*it)->CloseSocket();
            (*it)->Delete();
            }
          this->m_Sockets.clear();
          std::ostringstream msg;
          msg<<"Failed to connect to " <<  destinationIt->first
             << " port " <<  destinationIt->second;
          throw ExceptionWithMessage( msg.str() );
        }
        this->m_Sockets.push_back(socket);
      }
      // EndCodeSnippet
    }

    void Execute(itk::Object *caller, const itk::EventObject & event)
    {
      const itk::Object * constCaller = caller;
      this->Execute( constCaller, event );
    }

// BeginLatex
// In \emph{ToolUpdatedObserver::Excecute()}, we define the event
// handler to receive a transform, fill the OpenIGTLink message, and
// send it out.
// EndLatex

    void Execute(const itk::Object *caller, const itk::EventObject & event)
    {

      //if no destinations, just return
      if( m_Sockets.empty() )
        {
        return;
        }

      //do something only for the correct tool
      if( this->m_Tool.GetPointer() == caller )
        {               //the tool transform has been updated, get it
        if( dynamic_cast<const
            igstk::TrackerToolTransformUpdateEvent  *>( &event) )
          {                 //request to get the transform 
          this->m_Tool->RequestComputeTransformTo( this->m_World );
          //check that we got it
          if ( this->m_TransformObserver->GotTransform() )
            {
            // BeginCodeSnippet
            igstk::Transform transform =
              this->m_TransformObserver->GetTransform();
            igstk::Transform::VectorType t = transform.GetTranslation();
            igstk::Transform::VersorType r = transform.GetRotation();
            this->m_PositionMessage->SetPosition(t[0], t[1], t[2]);
            this->m_PositionMessage->SetQuaternion(r.GetX(), r.GetY(),
                                                     r.GetZ(), r.GetW());
            this->m_PositionMessage->Pack();

            std::vector< igtl::ClientSocket::Pointer >::iterator it; 
            for (it = this->m_Sockets.begin(); it != this->m_Sockets.end();
                 ++it)
              {
              (*it)->Send(this->m_PositionMessage->GetPackPointer(),
                          this->m_PositionMessage->GetPackSize());
              }
            // EndCodeSnippet
            }
          }
        }
      }

    private:
    std::string m_HostName;
    unsigned int m_PortNumber;  
    //we are interested in the tool location relative to the world's 
    //coordinate system
    igstk::SpatialObject::Pointer m_World;
    igstk::TransformObserver::Pointer m_TransformObserver; 
    igstk::TrackerTool::Pointer  m_Tool;

    //send data to these socket connections
    std::vector< igtl::ClientSocket::Pointer > m_Sockets;
    igtl::PositionMessage::Pointer m_PositionMessage;
    };


  igstk::OpenIGTLinkTrackerConfigurationFileReader::
    OpenIGTLinkConfigurationDataType *
    GetTrackerConfiguration( std::string &configurationFileName) 
    throw ( ExceptionWithMessage );

  igstk::TrackerController::Pointer m_TrackerController;
};
   

#endif //__OpenIGTLinkTrackingBroadcaster_h
